%% Plot Robot Movement
% function for plotting the robot movement at every step

function h  = plot_robot_movement(axes,h_prev,cur_r,cur_ang,polygon_width,color)
global workspace_data;    

    if nargin <= 4
        polygon_width = 2;
    end
    
    if nargin <= 5
        color = 'y';
    end
    
    polygon_height = polygon_width*2/3;
    
    if ~isempty(h_prev)
        delete(h_prev);
    end
    
    hold all
    
    cur_r(1) = cur_r(1)*workspace_data.px_to_m_x;
    cur_r(2) = cur_r(2)*workspace_data.px_to_m_y;
    y_rob = [cur_r(1)-cos(pi/2-cur_ang)*polygon_height/2,cur_r(1)+cos(pi/2-cur_ang)*polygon_height/2, cur_r(1)+cos(cur_ang)*polygon_width];
    x_rob = [cur_r(2)+sin(pi/2-cur_ang)*polygon_height/2, cur_r(2)-sin(pi/2-cur_ang)*polygon_height/2, cur_r(2)+sin(cur_ang)*polygon_width];
    
    h = fill(x_rob,y_rob,color);
    set(h,'Parent',axes);
    
    drawnow;
    hold off
end
